/*
PIO12Firefly_ProMini_LinearOF -- Prototype program to implement a simple 1-D optical
flow sensor using an Arduino Pro Mini (as sold by Sparkfun), a Centeye ArPrMi Vcs 1.0
shield, and a Centeye sensor head. The sensor head holds a FireflySmall vision chip
which has a press-molded PCX lens attached directly on-chip. The optical flow is sent
to the serial monitor as a display, with one line displayed every frame. (This limits
the frame rate of the sensor.)

Algorithm: One dimensional variation of Prof. Mandyam Srinivasan's "image interpolation"
algorithm. The vision chip is configured to implement vertical rectangle photoreceptors
(by binning a factor of 8 in the vertical direction).

Note: This code was written hastily and not a lot of effort was put in to optimizing
the speed. That would have been pointless because the majority of the delay is 
probably from the serial readout.

Written by Geoffrey L Barrows
Copyright (c) 2010
December 18, 2010
This code may be incorporate in any project, as is, but with no warranty or
guarantee whatsoever.

*/

/* Assumed PIO12 connections with vision chip
Analog1: Reconfiguerd digital for IO0
Analog2: Reconfigured digital for IO1
Digital7...3: IO7...2
Digital8: CS
Digital9: WR
Digital10: BS (monitor as input)
Analog0: Analog
*/

// PIO12 command codes for Firefly series vision chips
#define NBIAS 0x000
#define VREF 0x040
#define PRSUPPLY 0x0C0
#define ANALOGOUTBIAS 0x100
#define AMPCONF 0x200
#define AMPOP 0x240
#define FPCONF 0x280
#define COLSEL 0x400
#define ADCOP 0x600
#define RESB 0x640
#define REST 0x680
#define YUKNBIAS 0x6C0
#define CONNECTVDDA 0x700
#define BIASSWITCH1 0x740
#define BIASSWITCH2 0x780
#define BIASSWITCH3 0x7C0
#define ROWSEL 0x800
#define HSW 0xA00
#define VSW 0xC00
#define LINREG1 0xE00
#define LINREG2 0xE40
#define LINREG3 0xE80
#define LINREG4 0xEC0

#define BIASKEY1 0x0C
#define BIASKEY2 0x15
#define BIASKEY3 0x16

/*-------------------------------------------------------------*/
// PIO12 Functions -- interfacing with the Firefly chip

// Write a 12-bit command to PIO12, with two 6-bit arguments
void pio12write66(unsigned char hi, unsigned char lo) { 
  // Assume default e.g. CS=0, WNR=1
  // Step 1: Set IO0...IO5 to low hexabit
  PORTB = (PORTB&0xF8)|(hi&0x07); // IO0,IO1,IO2
  PORTC = (PORTC&0xF7)|(hi&0x08); // IO3
  PORTD = (PORTD&0xCF)|(hi&0x30); // IO4,IO5
  // Step 2: Set IO7 to high
  PORTD |= 0x80;
  // Step 3: Set CS high
  PORTC |= 0x02;
  // Step 4: Set CS low
  PORTC &= 0xFD;
  // Step 5: Set IO...IO5 to high hexabit
  PORTB = (PORTB&0xF8)|(lo&0x07); // IO0,IO1,IO2
  PORTC = (PORTC&0xF7)|(lo&0x08); // IO3
  PORTD = (PORTD&0xCF)|(lo&0x30); // IO4,IO5  
  // Step 6: Set IO7 to low
  PORTD &= 0x7F;
  // Step 7: Set CS high
  PORTC |= 0x02;
  // Step 8: Set CS low
  PORTC &= 0xFD;
}

// Write a command to PIO12, taking in 12-bit argument
void pio12write12(unsigned int hilo) { 
  unsigned char hi,lo;
  lo = hilo & 0x00FF;
  hi = (hilo>>6) & 0x00FF;
  pio12write66(hi,lo);
}

// Read a byte from the PIO12 interface
// Note: This function is unused and untested
unsigned char pio12read8(void) { 
  DDRD = 0x00; // set IO0...IO7 as input
  digitalWrite(9,LOW); // WNR = 0
  digitalWrite(8,HIGH); // CS = 1
  unsigned char bytein;
  bytein = PORTD;
  digitalWrite(8,LOW); // CS = 0
  digitalWrite(9,HIGH); // WNR = 1
  DDRD = 0xFF; // set IO0...IO7 as output
  return bytein;
}

// Digitize a byte from the analog input
unsigned int pio12getanalog(void) { 
  PORTC &= 0xFB; // WNR = 0
  PORTC |= 0x02; // CS = 1  
  unsigned int intin;
  delayMicroseconds(2);
  intin = analogRead(0);
  PORTC &= 0xFD; // CS = 0
  PORTC |= 0x04; // WNR = 1  
  return intin;
}

// Set up the Arduino for PIO12
void setupPIO12(void) { 
  DDRB |= 0x07; 
  DDRC |= 0x0E;
  DDRD |= 0xF0; 
  DDRD &= 0xF7;
  PORTC &= 0xFD; // CS=0
  PORTC |= 0x04; // WNR=1
}

// open or close vault - the vault must be opened to change
// sensitive parameters such as biases
void SetVault(unsigned char state) { 
  if (state==1) { 
    // open vault
    pio12write12(BIASSWITCH1+BIASKEY1);
    pio12write12(BIASSWITCH2+BIASKEY2);
    pio12write12(BIASSWITCH3+BIASKEY3);
  } else if (state==0) {
    // close vault
    pio12write12(BIASSWITCH1);
    pio12write12(BIASSWITCH2);
    pio12write12(BIASSWITCH3);    
  }
  if (0) {
    Serial.println();
    Serial.print("Target vault state = ");
    Serial.println(int(state));
    pinMode(3,INPUT);
    int bsm = digitalRead(3);
    Serial.print("Biasswitch = ");
    Serial.println(bsm);  
    delay(10);
  }
}

// Set up the Firefly biases for generic usage at 5V
void InitFireflyBiases(void) {
  SetVault(1);
  pio12write12(RESB+63);
  pio12write12(REST+0);  
  pio12write12(YUKNBIAS+63); //51
  pio12write12(VREF+62);
  pio12write12(NBIAS+56);
  pio12write12(PRSUPPLY+63);
  pio12write12(ANALOGOUTBIAS+56);
  pio12write12(CONNECTVDDA+2);
  SetVault(0);
}  
  
// Set up the Firefly chip for basic raw analog output
void InitFireflyBasicRaw(void) {
  pio12write12(VSW+0); // no vertical binning
  pio12write12(HSW+0); // no horizontal binning
  pio12write12(AMPCONF+12);
  pio12write12(AMPOP+1);
  pio12write12(FPCONF+4);
}


/*-------------------------------------------------------------------*/
// Global Variables
int rawimage[100]; // raw line image read from image sensor
int lpfimage[100]; // time domain low-passed image
int hpfimage1[100]; // time domain high-passed image #1
int hpfimage2[100]; // time domain high-passed image #2

int fpnadd[100]; // intentionally added fpn
int fpnstrength = 15; // added fpn strength

int lpalpha = 10; // "alpha" constant for low passing- in % so that 10 = 10% = 0.1

int rowstart = 60; // This is the row we will use
int colstart = 20; // This is the column from which we will start
int numcols = 60; // Number of columns we will read

int oflp = 3; // running average rate for optical flow, in tenths
long OF,OFLP; // optical flow and low-passed optical flow
  
int fff=1; // first frame flag

/*-------------------------------------------------------------------*/
// Main Functions

void setup() {
  setupPIO12();
  Serial.begin(9600); // set up serial stream to dump terminal
  InitFireflyBiases();
  InitFireflyBasicRaw();
  
  pio12write12(VSW+0x7F); // configure the array to make vertical rectangles
  for (char i=0; i<96; i+=4) {
    fpnadd[i] = 0;
    fpnadd[i+1] = fpnstrength;
    fpnadd[i+2] = fpnstrength;
    fpnadd[i+3] = 0;
  }
//  optional code to turn on linear voltage regulator    
//  pio12write12(LINREG1+0x24);
//  pio12write12(LINREG2+0x04);
//  pio12write12(LINREG3+0x05);
//  pio12write12(LINREG4+0x10);
}
  
void loop() {
  
  unsigned char r,c; // row and column variables
  unsigned char i,j;
  int pixval;
  
  // Set row to desired row
  pio12write12(ROWSEL+rowstart);

  // Step 1: Read in raw line image and generate high-passed images
  for (i=0; i!=numcols; ++i) {
    c = colstart + i; // determine column
    pio12write12(COLSEL+c);
    delayMicroseconds(10);
    pixval = pio12getanalog();    
    pixval = pixval << 4;
    if (fff) {
      lpfimage[i] = pixval;
      hpfimage2[i] = fpnadd[i];
    }
    // store value
    rawimage[i] = pixval;
    // low-passed image
    lpfimage[i] += lpalpha*(pixval - lpfimage[i])/100;
    // high-passed images
    hpfimage1[i] = hpfimage2[i];
    hpfimage2[i] = pixval - lpfimage[i] + fpnadd[i];
  }


  // Step 2: compute optical flow
  long top = 0;
  long bottom = 0;

  for (i=20; i<40; ++i) { // loop through pixels
    long fzf0 = hpfimage2[i] - hpfimage1[i];
    long f2f1 = hpfimage1[i+1] - hpfimage1[i-1];
    top += fzf0 * f2f1;
    bottom += f2f1 * f2f1;
  }
  OF = 400 * top / bottom;
    
  OFLP += oflp*(OF-OFLP)/10;
//  OFLP = OF; 
  
//  Serial.print(OF);
//  Serial.print(" "); 
//  Serial.println(OFLP);
  
  // Step 3: display optical flow on serial output
  if (OFLP<15 && OFLP>-15) {
    Serial.println("        ");
  } else if (OFLP>0) {
    if (OFLP<50)
      Serial.println("   R    ");
    else if (OFLP<100)
      Serial.println("   RR   ");
    else if (OFLP<150)
      Serial.println("  RRR   ");
    else if (OFLP<200)
      Serial.println("  RRRR  ");
    else if (OFLP<250)
      Serial.println(" RRRRR  ");
    else if (OFLP<300)
      Serial.println(" RRRRRR ");
    else if (OFLP<350)
      Serial.println("RRRRRRR ");
    else
      Serial.println("RRRRRRRR");
  } else { // OF<0
    if (OFLP>-50)
      Serial.println("   |    ");
    else if (OFLP>-100)
      Serial.println("   []   ");
    else if (OFLP>-150)
      Serial.println("  [|]   ");
    else if (OFLP>-200)
      Serial.println("  [[]]  ");
    else if (OFLP>-250)
      Serial.println(" [[ ]]  ");
    else if (OFLP>-300)
      Serial.println(" [[  ]] ");
    else if (OFLP>-350)
      Serial.println("[[   ]] ");
    else
      Serial.println("[[    ]]");
  }
    
  fff = 0;
    
}
  
  
